// <copyright file="PathfinderStandard.cs" company="Gustavo Franco">
// Copyright (c) 2006 All Right Reserved
// </copyright>
// <author>Gustavo Franco</author>
// <email>gustavo_franco@hotmail.com</email>
// <date>2006</date>
// <summary>Path finder algorithm which uses the standard method for pathfinding i.e.: Slower but more accurate.</summary>

// THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
// KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
// PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
// REMAINS UNCHANGED.

// Email:  gustavo_franco@hotmail.com

// Copyright (C) 2006 Franco, Gustavo

// Work done by Robert Tenney on this library: PathFinder, is based off of the previous work
// done by: Franco, Gustavo.

using System;
using System.Collections.Generic;
using System.Drawing;

namespace TenneySoftware.Engine.Algorithms.Pathfinder {
    /// <summary>
    /// Path finder algorithm which uses the standard method for pathfinding i.e.: Slower but more accurate.
    /// </summary>
    [Author("Franco, Gustavo")]
    public class PathfinderStandard : IPathfinder {
        #region Variables Declaration
        /// <summary>
        /// Grid for pathfinding in.
        /// </summary>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1814:PreferJaggedArraysOverMultidimensional", MessageId = "Member", Justification = "Multidimensional preferred.")]
        private byte[,] grid = null;

        /// <summary>
        /// Queue of open location.
        /// </summary>
        private PriorityQueueB<PathfinderNode> open = new PriorityQueueB<PathfinderNode>(new ComparePFNode());

        /// <summary>
        /// Collection of closed nodes.
        /// </summary>
        private PathfinderNodeCollection close = new PathfinderNodeCollection();

        /// <summary>
        /// Was stop called.
        /// </summary>
        private bool stop;

        /// <summary>
        /// Are we stopped.
        /// </summary>
        private bool stopped = true;

        /// <summary>
        /// Horizontal line being scanned.
        /// </summary>
        private int horizontal;

        /// <summary>
        /// Formula to use for pathfinding.
        /// </summary>
        private HeuristicFormula formula = HeuristicFormula.Manhattan;

        /// <summary>
        /// Are we allowing diagonals.
        /// </summary>
        private bool diagonals = true;

        /// <summary>
        /// Heuristic estimate value.
        /// </summary>
        private int heuristicEstimate = 2;

        /// <summary>
        /// Should we make directional changes heavier.
        /// </summary>
        private bool punishChangeDirection;

        /// <summary>
        /// Should we use a tibreaker in the event of a tie.
        /// </summary>
        private bool tieBreaker;

        /// <summary>
        /// Should we make the diagonals heavier.
        /// </summary>
        private bool heavyDiagonals;

        /// <summary>
        /// The farthest distance away to search.
        /// </summary>
        private int searchLimit = 2000;

        /// <summary>
        /// Time taken to complete pathfinding.
        /// </summary>
        private double completedTime;
        #endregion

        #region Constructors
        /// <summary>
        /// Initializes a new instance of the PathfinderStandard class.
        /// </summary>
        /// <param name="grid">Pathfinding field.</param>
        public PathfinderStandard(byte[,] grid) {
            if (grid == null) {
                throw new ArgumentNullException("grid", "Grid cannot be null");
            }
            
            this.grid = grid;
        }
        #endregion

        #region Properties
        /// <summary>
        /// Gets a value indicating whether the algorithm was stopped or not.
        /// </summary>
        /// <value>
        /// A value indicating whether the algorithm was stopped or not.
        /// </value>
        public bool Stopped {
            get {
                return this.stopped;
            }
        }

        /// <summary>
        /// Gets or sets the formula used for heuristics.
        /// </summary>
        /// <value>
        /// The formula used for heuristics.
        /// </value>
        public HeuristicFormula Formula {
            get {
                return this.formula;
            }
            
            set {
                this.formula = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to allow diagonals.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to allow diagonals.
        /// </value>
        public bool Diagonals {
            get {
                return this.diagonals;
            }
            
            set {
                this.diagonals = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to score diagonals heavily.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to score diagonals heavily.
        /// </value>
        public bool HeavyDiagonals {
            get {
                return this.heavyDiagonals;
            }
            
            set {
                this.heavyDiagonals = value;
            }
        }

        /// <summary>
        /// Gets or sets a value for use by the heuristic formula for it's estimation.
        /// </summary>
        /// <value>
        /// A value for use by the heuristic formula for it's estimation.
        /// </value>
        public int HeuristicEstimate {
            get {
                return this.heuristicEstimate;
            }
            
            set {
                this.heuristicEstimate = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to score directional changes more heavily.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to score directional changes more heavily.
        /// </value>
        public bool PunishChangeDirection {
            get {
                return this.punishChangeDirection;
            }
            
            set {
                this.punishChangeDirection = value;
            }
        }

        /// <summary>
        /// Gets or sets a value indicating whether or not to perform additional math to break the tie.
        /// </summary>
        /// <value>
        /// A value indicating whether or not to perform additional math to break the tie.
        /// </value>
        public bool Tiebreaker {
            get {
                return this.tieBreaker;
            }
            
            set {
                this.tieBreaker = value;
            }
        }

        /// <summary>
        /// Gets or sets a value to set the search limit to.
        /// </summary>
        /// <value>
        /// A value to set the search limit to.
        /// </value>
        public int SearchLimit {
            get {
                return this.searchLimit;
            }
            
            set {
                this.searchLimit = value;
            }
        }

        /// <summary>
        /// Gets or sets the time taken to complete the algorithm.
        /// </summary>
        /// <value>
        /// The time taken to complete the algorithm.
        /// </value>
        public double CompletedTime {
            get {
                return this.completedTime;
            }
            
            set {
                this.completedTime = value;
            }
        }
        #endregion

        #region Methods
        /// <summary>
        /// Causes the path finder algorithm to stop if it is currently running.
        /// </summary>
        public void FindPathStop() {
            this.stop = true;
        }

        /// <summary>
        /// Starts the path finding algorithm.
        /// </summary>
        /// <param name="start">Start of the path.</param>
        /// <param name="destination">End of the path.</param>
        /// <returns>A list of nodes with the path finder's solution.</returns>
        [System.Diagnostics.CodeAnalysis.SuppressMessage("Microsoft.Performance", "CA1814:PreferJaggedArraysOverMultidimensional", MessageId = "Body", Justification = "Multidimensional preferred.")]
        public PathfinderNodeCollection FindPath(Point start, Point destination) {
            HighResolutionTimer.Start();

            PathfinderNode parentNode = new PathfinderNode();
            bool found = false;
            int gridX = this.grid.GetUpperBound(0);
            int gridY = this.grid.GetUpperBound(1);

            this.stop = false;
            this.stopped = false;
            this.open.Clear();
            this.close.Clear();

            sbyte[,] direction;
            if (this.diagonals) {
                direction = new sbyte[8, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 } };
            } else {
                direction = new sbyte[4, 2] { { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 } };
            }
            
            parentNode.G = 0;
            parentNode.H = this.heuristicEstimate;
            parentNode.F = parentNode.G + parentNode.H;
            parentNode.X = start.X;
            parentNode.Y = start.Y;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            this.open.Push(parentNode);
            while (this.open.Count > 0 && !this.stop) {
                parentNode = this.open.Pop();
                if (parentNode.X == destination.X && parentNode.Y == destination.Y) {
                    this.close.Add(parentNode);
                    found = true;
                    break;
                }

                if (this.close.Count > this.searchLimit) {
                    this.stopped = true;
                    return null;
                }

                if (this.punishChangeDirection) {
                    this.horizontal = (parentNode.X - parentNode.PX);
                }
                
                // Lets calculate each successors
                for (int i = 0; i < (this.diagonals ? 8 : 4); i++) {
                    PathfinderNode newNode = new PathfinderNode();
                    newNode.X = parentNode.X + direction[i, 0];
                    newNode.Y = parentNode.Y + direction[i, 1];

                    if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY) {
                        continue;
                    }

                    int newG;
                    if (this.heavyDiagonals && i > 3) {
                        newG = parentNode.G + (int)(this.grid[newNode.X, newNode.Y] * 2.41);
                    } else {
                        newG = parentNode.G + this.grid[newNode.X, newNode.Y];
                    }

                    if (newG == parentNode.G) {
                        // Unbrekeable
                        continue;
                    }

                    if (this.punishChangeDirection) {
                        if ((newNode.X - parentNode.X) != 0) {
                            if (this.horizontal == 0) {
                                newG += 20;
                            }
                        }
                        
                        if ((newNode.Y - parentNode.Y) != 0) {
                            if (this.horizontal != 0) {
                                newG += 20;
                            }
                        }
                    }

                    int foundInOpenIndex = -1;
                    for (int j = 0; j < this.open.Count; j++) {
                        if (this.open[j].X == newNode.X && this.open[j].Y == newNode.Y) {
                            foundInOpenIndex = j;
                            break;
                        }
                    }
                    
                    if (foundInOpenIndex != -1 && this.open[foundInOpenIndex].G <= newG) {
                        continue;
                    }
                    
                    int foundInCloseIndex = -1;
                    for (int j = 0; j < this.close.Count; j++) {
                        if (this.close[j].X == newNode.X && this.close[j].Y == newNode.Y) {
                            foundInCloseIndex = j;
                            break;
                        }
                    }
                    
                    if (foundInCloseIndex != -1 && this.close[foundInCloseIndex].G <= newG) {
                        continue;
                    }
                    
                    newNode.PX = parentNode.X;
                    newNode.PY = parentNode.Y;
                    newNode.G = newG;

                    switch (this.formula) {
                        default:
                        case HeuristicFormula.Manhattan:
                            newNode.H = this.heuristicEstimate * (Math.Abs(newNode.X - destination.X) + Math.Abs(newNode.Y - destination.Y));
                            break;
                        case HeuristicFormula.MaxDxDy:
                            newNode.H = this.heuristicEstimate * Math.Max(Math.Abs(newNode.X - destination.X), Math.Abs(newNode.Y - destination.Y));
                            break;
                        case HeuristicFormula.DiagonalShortcut:
                            int horizontalDiagonal = Math.Min(Math.Abs(newNode.X - destination.X), Math.Abs(newNode.Y - destination.Y));
                            int horizontalStraight = (Math.Abs(newNode.X - destination.X) + Math.Abs(newNode.Y - destination.Y));
                            newNode.H = ((this.heuristicEstimate * 2) * horizontalDiagonal) + (this.heuristicEstimate * (horizontalStraight - (2 * horizontalDiagonal)));
                            break;
                        case HeuristicFormula.Euclidean:
                            newNode.H = (int)(this.heuristicEstimate * Math.Sqrt(Math.Pow((newNode.X - destination.X), 2) + Math.Pow((newNode.Y - destination.Y), 2)));
                            break;
                        case HeuristicFormula.EuclideanNoSqr:
                            newNode.H = (int)(this.heuristicEstimate * (Math.Pow((newNode.X - destination.X), 2) + Math.Pow((newNode.Y - destination.Y), 2)));
                            break;
                        case HeuristicFormula.Custom1:
                            Point dxy = new Point(Math.Abs(destination.X - newNode.X), Math.Abs(destination.Y - newNode.Y));
                            int orthogonal = Math.Abs(dxy.X - dxy.Y);
                            int diagonal = Math.Abs(((dxy.X + dxy.Y) - orthogonal) / 2);
                            newNode.H = this.heuristicEstimate * (diagonal + orthogonal + dxy.X + dxy.Y);
                            break;
                    }
                    
                    if (this.tieBreaker) {
                        int dx1 = parentNode.X - destination.X;
                        int dy1 = parentNode.Y - destination.Y;
                        int dx2 = start.X - destination.X;
                        int dy2 = start.Y - destination.Y;
                        int cross = Math.Abs((dx1 * dy2) - (dx2 * dy1));
                        newNode.H = (int)(newNode.H + (cross * 0.001));
                    }
                    
                    newNode.F = newNode.G + newNode.H;
                    this.open.Push(newNode);
                }

                this.close.Add(parentNode);
            }

            this.completedTime = HighResolutionTimer.GetTime();
            if (found) {
                PathfinderNode finderNode = this.close[this.close.Count - 1];
                for (int i = this.close.Count - 1; i >= 0; i--) {
                    if (finderNode.PX == this.close[i].X && finderNode.PY == this.close[i].Y || i == this.close.Count - 1) {
                        finderNode = this.close[i];
                    } else {
                        this.close.RemoveAt(i);
                    }
                }
                
                this.stopped = true;
                return this.close;
            }
            
            this.stopped = true;
            return null;
        }
        #endregion

        #region Inner Classes
        /// <summary>
        /// Pathfinder node comparer.
        /// </summary>
        [Author("Franco, Gustavo")]
        internal class ComparePFNode : IComparer<PathfinderNode> {
            #region IComparer Members
            /// <summary>
            /// Compare two nodes with each other.
            /// </summary>
            /// <param name="x">First node.</param>
            /// <param name="y">Second node.</param>
            /// <returns>If a is less then b then a value less than zero will return. If equal, then zero will return.
            /// If greater then a value greater than zero will be returned.</returns>
            public int Compare(PathfinderNode x, PathfinderNode y) {
                if (x.F > y.F) {
                    return 1;
                } else if (x.F < y.F) {
                    return -1;
                }
                
                return 0;
            }
            #endregion
        }
        #endregion
    }
}
